#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "can_driver.h"
#include <pthread.h>
#include "linux/can.h"
#include "linux/can/raw.h"
#include "net/if.h"

#include "can_driver.h"



NodeInfor nodeinfor[NODE_NUM]={0};



static int debugMsg(Message*msg)
{
	int cnt=0;
	
	if(msg == NULL)
		return -1;

	printf("cob_id:0x%04x\n",msg->cob_id);
	printf("len:%d\n",msg->len);
	for(cnt=0;cnt<msg->len;cnt++)
	{
		printf("data[%d]:0x%02x ",cnt,msg->data[cnt]);
	}
	printf("\n");

	return 0;
}

void *canDispatch(void *fd0)
{
	struct can_frame frame;
	int res = 0;
	UNS32 cob_id=0;
	UNS32 node_id=0;
	int cnt;
	unsigned globalCnt=0;
	
	while(1)
	{
		//disable the PDO 
		for(cnt=0;cnt<NODE_NUM;cnt++)
		{
			if((nodeinfor[cnt].state == NODE_VALID)&&(nodeinfor[cnt].PDOFLAG == ENABLE))
			{
				canMotor_DisbalePDOs(fd0,nodeinfor[cnt].node_id);
				nodeinfor[cnt].PDOFLAG = DISABLE;
			}
		}


	
can_rcv:	
	    res = recv (*(int *) fd0, &frame, sizeof (frame), 0);
	    if (res <= 0)
	    {
	    	usleep(10*1000);
			continue;
	    }



		//printf("recv:0x%x data[0]:0x%02x\n",frame.can_id,frame.data[0]);
		cob_id = frame.can_id&0xF00;
		node_id = frame.can_id&0xFF;

		//printf("cob_id:0x%x\n",cob_id);
		switch(cob_id)
		{
			case 0x700:
				//printf("case 0x700\n");				
				for(cnt=0;cnt<NODE_NUM;cnt++)
				{
					if(node_id == nodeinfor[cnt].node_id)
					{
						if(frame.data[0] == 0x7F)
						{
							nodeinfor[cnt].state = NODE_INVALID;
							canMotor_EnterOperational(fd0,node_id);
							goto can_rcv;
						}
						else if(frame.data[0] == 0x05)
						{
							//printf("node:0x%x is valid\n",node_id);
							nodeinfor[cnt].state = NODE_VALID;
							goto can_rcv;
						}
					}			
				}				
			break;
			case 0x500:
				/*
				if((frame.data[2]<<8|frame.data[1])==0x6041)
				{
					printf("rcv 0x581,index:0x%x\n",(frame.data[2]<<8|frame.data[1]));
					printf("state:0x%x\n",frame.data[5]<<8|frame.data[4]);

				}
				*/
				
				for(cnt=0;cnt<NODE_NUM;cnt++)
				{
					if((frame.data[2]<<8|frame.data[1])==(0x6040+nodeinfor[cnt].node_id))
					{
						//printf("rcv 0x581,index:0x%x\n",(frame.data[2]<<8|frame.data[1]));
						//printf("state:0x%x\n",frame.data[5]<<8|frame.data[4]);
						nodeinfor[cnt].stateReg= frame.data[5]<<8|frame.data[4];
						printf("node:0x%02x state:0x%x\n",nodeinfor[cnt].node_id,nodeinfor[cnt].stateReg);
					}

					
				}


				
			break;
			
			
		}


		//usleep(10*1000);
		//globalCnt++;
		//if(globalCnt >=10)
		{
			globalCnt=0;

			//query the motor state at a fix cycyle,such as 100ms
			for(cnt=0;cnt<NODE_NUM;cnt++)
			{
				if(nodeinfor[cnt].state == NODE_VALID)
					canMotor_QueryStatus(fd0,nodeinfor[cnt].node_id);				
			}
			
			
		}
		
	}

}


int main(int argc,char**argv)
{
	CAN_HANDLE s=NULL;
	Message msg,msg_rx;
	int status=0;
	int cnt=0;
	int error=0;
	int tmp=0;
	pthread_t threadId;

	/*node init*/
	nodeinfor[0].node_id = 0x01;
	nodeinfor[0].state = NODE_UNDEF;
	nodeinfor[0].PDOFLAG = ENABLE;

	nodeinfor[1].node_id = 0x02;
	nodeinfor[1].state = NODE_UNDEF;
	nodeinfor[1].PDOFLAG = ENABLE;

	nodeinfor[2].node_id = 0x03;
	nodeinfor[2].state = NODE_UNDEF;
	nodeinfor[2].PDOFLAG = ENABLE;



	
	canInit_driver();

	s = canOpen_driver();

	//canMotor_FilterConfig(s,nodeinfor[0].node_id);
	//canMotor_FilterConfig(s,nodeinfor[1].node_id);
	//canMotor_FilterConfig(s,nodeinfor[2].node_id);
	canMotor_Reset(s,nodeinfor[0].node_id);
	canMotor_Reset(s,nodeinfor[1].node_id);
	canMotor_Reset(s,nodeinfor[2].node_id);


	
	error=pthread_create(&threadId,NULL,canDispatch,(void*)s);	
	
	while(1)
	{
			
	/*
		canReceive_driver(s,&msg_rx);
		canMotor_Parse(&msg_rx);
		*/

		if(nodeinfor[0].state == NODE_VALID)
		{
			canMotor_HomeModeTest(s,nodeinfor[0].node_id);
			
			tmp = nodeinfor[0].stateReg&(0x1<<12);
			printf("tmp:0x%x\n",tmp);
			while(tmp == 0)
			{
				usleep(1*1000);
			}

		}



		
		sleep(5);
	}
	return 0;
}
